#include "eeprom.h"
//#include <avr/io.h>
#include <avr/eeprom.h>


#define PID_TI_HWADDR 0
#define PID_TD_HWADDR 4
#define PID_KP_HWADDR 8

#define SONARCOUNT_HWADDR 12

union
{
	float fv;
	unsigned char cv[4];
}f2b;

//TODO: En avr-libc 1.7.0 ya hay metodos para guardar floats, eeprom_write_float. Ademas lo mas importante es que hay forma 
// de verificar si el dato es  o no es el mismo, para no reescribir innecesariamente la eeprom, esto es con las funciones eeprom_update.

void eeprom_writePIDParam(unsigned char PIDparam, float value)
{
	unsigned char PARAM_HWADDR = 0;

	f2b.fv = value;
	
	//definir la direccion donde almacenar los datos
	if(PIDparam == PID_TI)
		PARAM_HWADDR = PID_TI_HWADDR;
	else if(PIDparam == PID_TD)
		PARAM_HWADDR = PID_TD_HWADDR;	
	else if(PIDparam == PID_KP)
		PARAM_HWADDR = PID_KP_HWADDR;
		
	eeprom_write_byte((unsigned char*)(PARAM_HWADDR),f2b.cv[0]);
	eeprom_write_byte((unsigned char*)(PARAM_HWADDR+1),f2b.cv[1]);
	eeprom_write_byte((unsigned char*)(PARAM_HWADDR+2),f2b.cv[2]);
	eeprom_write_byte((unsigned char*)(PARAM_HWADDR+3),f2b.cv[3]);

}

void eeprom_writeSonarCount(unsigned char sonarCount)
{
	eeprom_write_byte((unsigned char*)(SONARCOUNT_HWADDR),sonarCount);
}



float eeprom_readPIDParam(unsigned char PIDparam)
{
	unsigned char PARAM_HWADDR = 0;
	
	//definir la direccion donde almacenar los datos
	if(PIDparam == PID_TI)
		PARAM_HWADDR = PID_TI_HWADDR;
	else if(PIDparam == PID_TD)
		PARAM_HWADDR = PID_TD_HWADDR;	
	else if(PIDparam == PID_KP)
		PARAM_HWADDR = PID_KP_HWADDR;
		
	f2b.cv[0] = eeprom_read_byte((unsigned char*)(PARAM_HWADDR+0));
	f2b.cv[1] = eeprom_read_byte((unsigned char*)(PARAM_HWADDR+1));
	f2b.cv[2] = eeprom_read_byte((unsigned char*)(PARAM_HWADDR+2));
	f2b.cv[3] = eeprom_read_byte((unsigned char*)(PARAM_HWADDR+3));
	
	return f2b.fv;
	
}

unsigned char eeprom_readSonarCount()
{
	return eeprom_read_byte((unsigned char*)(SONARCOUNT_HWADDR));
}


